package at.fhooe.engine.pathfinding;

import java.awt.Point;
import java.util.ArrayList;

import at.fhooe.enginge.pathfinding.help.AandBZeroException;
import at.fhooe.enginge.pathfinding.help.AlgebraicLine;

public class AStar {
	private ArrayList<AStarNode> opened = new ArrayList<AStarNode>();
	private ArrayList<AStarNode> closed = new ArrayList<AStarNode>();
	private ArrayList<AStarNode> bestList = new ArrayList<AStarNode>();
	
	private AStarNode[][] level;
	private Point goal;
	private Point start;
	
	public void createLevelForAStar(int[][] level, Point start, Point goal) {
		this.goal = (Point) goal.clone();
		this.start = (Point) start.clone();
		this.level = new AStarNode[level.length][level[0].length];
		for(int y = 0; y < this.level.length; y++) {
			for(int x = 0; x < this.level[0].length; x++) {
				System.out.print(level[x][y]);
				int costToFinish = this.getCostToGoal(x, y);
				if(level[x][y] == BalancingPf.LEVEL_WALL) {
					this.level[y][x] = new AStarNode(x, y, BalancingPf.COST_WALL, costToFinish);
				} else if (level[x][y] == BalancingPf.LEVEL_FREE) {
					this.level[y][x] = new AStarNode(x, y, BalancingPf.COST_FREE, costToFinish);
				}
			}
			System.out.println();
		}
		System.out.println("AStar.java: startx: "+start.x+" starty: "+start.y+" goalx: "+goal.x+" goaly: "+goal.y);
		if(this.level[this.start.y][this.start.x] != null) {
			this.findBestWay();
		}
	}
	
	private void findBestWay() {
		this.opened = new ArrayList<AStarNode>();
		this.closed = new ArrayList<AStarNode>();
		this.bestList = new ArrayList<AStarNode>();
		
		this.opened.add(this.level[this.start.y][this.start.x]);
	}
	
	public void makeAllSteps() {
		while (this.opened.size() > 0) {
			this.makeStep();
		}
	}
	
	public void makeStep() {
		if (this.opened.size() > 0) {
			AStarNode node = this.opened.get(0);
			this.opened.remove(node);
			this.closed.add(node);
			
			int x = node.getX();
			int y = node.getY();
			if ((this.goal.x == x) && (this.goal.y == y)) {
				this.addDirectionToBestWay(this.goal.x, this.goal.y);
				return;
			} else {
				this.checkNewNode(node, x, y, -1, 0);
				this.checkNewNode(node, x, y, +1, 0);
				this.checkNewNode(node, x, y, 0, -1);
				this.checkNewNode(node, x, y, 0, 1);
			}
		} 
	}
	
	private void checkNewNode(AStarNode node, int x, int y, int changeX, int changeY) {
		if ((x + changeX >= 0) && (x + changeX < this.level[0].length) &&
			(y + changeY >= 0) && (y + changeY < this.level.length) && 
			(this.level[y+changeY][x+changeX] != null)) {
			AStarNode nextNode = this.level[y+changeY][x+changeX];
			int newCost = node.getCost() + nextNode.getMyCost();
			if (this.closed.indexOf(nextNode) >= 0) {
				if (newCost < nextNode.getCost()) {
					this.removeAndAdd(node, nextNode, newCost);
				}
			} else if (this.opened.indexOf(nextNode) >= 0) {
				if (newCost < nextNode.getCost()) {
					this.removeAndAdd(node, nextNode, newCost);
				}
			} else {
				this.removeAndAdd(node, nextNode, newCost);
			}
		}
	}
	
	private void removeAndAdd(AStarNode node, AStarNode nextNode, int newCost) {
		this.closed.remove(nextNode);
		this.opened.remove(nextNode);
		nextNode.setParent(node);
		nextNode.setCost(newCost);
		this.addToOpen(nextNode);
	}
	
	private void addToOpen(AStarNode node) {
		if (this.opened.size() <= 0) {
			this.opened.add(node);
		} else {
			for (int i = 0; i < this.opened.size(); i++) {
				if (this.opened.get(i).getAllCosts() > node.getAllCosts()) {
					this.opened.add(i, node);
					return;
				} else if (this.opened.get(i).getAllCosts() == node.getAllCosts()) {
					if (this.opened.get(i).getMyCostToFinish() > node.getMyCostToFinish()) {
						this.opened.add(i, node);
						return;
					}
				}
			}
			this.opened.add(node);
		}
	}
	
	private int getCostToGoal(int x, int y) {
		return Math.abs(this.goal.x - x) + Math.abs(this.goal.y - y);
	}
	
	private void addDirectionToBestWay(final int x, final int y) {
		if ((x < 0) || (y < 0)) {
			return;
		}
		AStarNode node = this.level[y][x];
		this.bestList.add(node);
		if ((this.start == null) || (node == null) || ((x == this.start.x) && (y == this.start.y)) || (node.getParent() == null)) {
			return;
		}
		if (this.bestList.size() < this.level.length * this.level[0].length) {
			if ((node.getParent().getX() == this.start.x) && (node.getParent().getY() == this.start.y) && (this.level[this.start.y][this.start.x].getMyCost() < 200)) {
				if (this.level[y][x].getMyCost() >= 200) {
					this.bestList.clear();
				}
				return;
			}
			this.addDirectionToBestWay(node.getParent().getX(), node.getParent().getY());
		} else {
			return;
		}
	}
	
	public void drawLevel() {
		String[][] out = new String[this.level.length][this.level[0].length];
		for (int y = 0; y < this.level.length; y++) {
			for (int x = 0; x < this.level[0].length; x++) {
				if (((this.start.x != x) || (this.start.y != y)) && ((this.goal.x != x) || (this.goal.y != y))) {
					
					if (this.level[y][x].getType() == BalancingPf.COST_FREE) {
						out[x][y] = "+";
					} else if (this.level[y][x].getType() == BalancingPf.COST_WALL) {
						out[x][y] = "#";
					}
				} else {
					if ((this.start.x == x) && (this.start.y == y)) {
						out[x][y] = "P";
					} else if ((this.goal.x == x) && (this.goal.y == y)) {
						out[x][y] = "G";
					}
				}
			}
		}
		
		for(int i = 1; i < bestList.size(); i++) {
			int a = bestList.size()-i;
			out[bestList.get(i).getX()][bestList.get(i).getY()] = ""+a;
		}
		
		for (int y = 0; y < this.level.length; y++) {
			for (int x = 0; x < this.level[0].length; x++) {
				System.out.print(out[x][y]);
			}
			System.out.println();
		}
	}
	
	public ArrayList<Point> getListOfWayPoints(double xMul, double yMul) {
		ArrayList<Point> ret = new ArrayList<Point>();
		for(int i = bestList.size()-1; i > -1; i--) {
			AStarNode n = bestList.get(i);
			ret.add(new Point((int)((n.getX()+0.5)*xMul), (int)((n.getY()+0.5)*yMul)));
		}
		
		return ret;
	}

	public ArrayList<Point> getOptimizedListOfWayPoints(double xMul, double yMul) {
		System.out.println("Linie optimieren");
		ArrayList<Point> ret = new ArrayList<Point>();
		for(int i = bestList.size()-1; i > -1; i--) {
			AStarNode n = bestList.get(i);
			ret.add(new Point((int)((n.getX()+0.5)*xMul), (int)((n.getY()+0.5)*yMul)));
		}
		int startPoint = 0;
		ArrayList<Point> del = new ArrayList<Point>();
		try {
			while(startPoint+1 < ret.size()) {
				AlgebraicLine start = new AlgebraicLine(ret.get(startPoint), ret.get(startPoint+1));
				int linePoint2 = startPoint+1;
				for(int i = startPoint+2; i < ret.size(); i++) {
					if(start.distance(ret.get(i)) < 0.5 && start.distance(ret.get(i)) > -0.5) {
						del.add(ret.get(linePoint2));
						start = new AlgebraicLine(ret.get(startPoint), ret.get(i));
						linePoint2 = i;
					} else {
						break;
					}
				}
				
				startPoint = linePoint2;
			}
			for(Point e : del) {
				ret.remove(e);
			}
		} catch (AandBZeroException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		return ret;
	}
}
